using UnityEngine;
using System.Collections;
using System.Collections.Generic;

public class MathUtils
{
	public static float LengthSQ ( Vector2 a )
	{
		return (a.x * a.x + a.y * a.y );
	}

	public static float LengthSQ ( Vector3 a )
	{
		return (a.x * a.x + a.y * a.y + a.z * a.z);
	}

	public static float DistanceSQ ( Vector3 a, Vector3 b )
	{
		return MathUtils.LengthSQ( b - a );
	}

	public static float GetQuatLength(Quaternion q)
	{
		return Mathf.Sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
	}

	public static Quaternion GetQuatConjugate(Quaternion q)
	{
		return new Quaternion(-q.x, -q.y, -q.z, q.w);
	}

	/// <summary>
	/// Logarithm of a unit quaternion. The result is not necessary a unit quaternion.
	/// </summary>
	public static Quaternion GetQuatLog(Quaternion q)
	{
		Quaternion res = q;
		res.w = 0;

		if (Mathf.Abs(q.w) < 1.0f)
		{
			float theta = Mathf.Acos(q.w);
			float sin_theta = Mathf.Sin(theta);

			if (Mathf.Abs(sin_theta) > 0.0001)
			{
				float coef = theta / sin_theta;
				res.x = q.x * coef;
				res.y = q.y * coef;
				res.z = q.z * coef;
			}
		}

		return res;
	}

	public static Quaternion GetQuatExp(Quaternion q)
	{
		Quaternion res = q;

		float fAngle = Mathf.Sqrt(q.x * q.x + q.y * q.y + q.z * q.z);
		float fSin = Mathf.Sin(fAngle);

		res.w = Mathf.Cos(fAngle);

		if (Mathf.Abs(fSin) > 0.0001)
		{
			float coef = fSin / fAngle;
			res.x = coef * q.x;
			res.y = coef * q.y;
			res.z = coef * q.z;
		}

		return res;
	}

	/// <summary>
	/// SQUAD Spherical Quadrangle interpolation [Shoe87]
	/// </summary>
	public static Quaternion GetQuatSquad(float t, Quaternion q0, Quaternion q1, Quaternion a0, Quaternion a1)
	{
		float slerpT = 2.0f * t * (1.0f - t);

		Quaternion slerpP = Slerp(q0, q1, t);
		Quaternion slerpQ = Slerp(a0, a1, t);

		return Slerp(slerpP, slerpQ, slerpT);
	}

	public static Quaternion GetSquadIntermediate(Quaternion q0, Quaternion q1, Quaternion q2)
	{
		Quaternion q1Inv = GetQuatConjugate(q1);
		Quaternion p0 = GetQuatLog(q1Inv * q0);
		Quaternion p2 = GetQuatLog(q1Inv * q2);
		Quaternion sum = new Quaternion(-0.25f * (p0.x + p2.x), -0.25f * (p0.y + p2.y), -0.25f * (p0.z + p2.z), -0.25f * (p0.w + p2.w));

		return q1 * GetQuatExp(sum);
	}

	/// <summary>
	/// Smooths the input parameter t.
	/// If less than k1 ir greater than k2, it uses a sin.
	/// Between k1 and k2 it uses linear interp.
	/// </summary>
	public static float Ease(float t, float k1, float k2)
	{
		float f; float s;

		f = k1 * 2 / Mathf.PI + k2 - k1 + (1.0f - k2) * 2 / Mathf.PI;

		if (t < k1)
		{
			s = k1 * (2 / Mathf.PI) * (Mathf.Sin((t / k1) * Mathf.PI / 2 - Mathf.PI / 2) + 1);
		}
		else
			if (t < k2)
			{
				s = (2 * k1 / Mathf.PI + t - k1);
			}
			else
			{
				s = 2 * k1 / Mathf.PI + k2 - k1 + ((1 - k2) * (2 / Mathf.PI)) * Mathf.Sin(((t - k2) / (1.0f - k2)) * Mathf.PI / 2);
			}

		return (s / f);
	}

	/// <summary>
	/// We need this because Quaternion.Slerp always uses the shortest arc.
	/// </summary>
	public static Quaternion Slerp(Quaternion p, Quaternion q, float t)
	{
		Quaternion ret;

		float fCos = Quaternion.Dot(p, q);

		if ((1.0f + fCos) > 0.00001)
		{
			float fCoeff0, fCoeff1;

			if ((1.0f - fCos) > 0.00001)
			{
				float omega = Mathf.Acos(fCos);
				float invSin = 1.0f / Mathf.Sin(omega);
				fCoeff0 = Mathf.Sin((1.0f - t) * omega) * invSin;
				fCoeff1 = Mathf.Sin(t * omega) * invSin;
			}
			else
			{
				fCoeff0 = 1.0f - t;
				fCoeff1 = t;
			}

			ret.x = fCoeff0 * p.x + fCoeff1 * q.x;
			ret.y = fCoeff0 * p.y + fCoeff1 * q.y;
			ret.z = fCoeff0 * p.z + fCoeff1 * q.z;
			ret.w = fCoeff0 * p.w + fCoeff1 * q.w;
		}
		else
		{
			float fCoeff0 = Mathf.Sin((1.0f - t) * Mathf.PI * 0.5f);
			float fCoeff1 = Mathf.Sin(t * Mathf.PI * 0.5f);

			ret.x = fCoeff0 * p.x - fCoeff1 * p.y;
			ret.y = fCoeff0 * p.y + fCoeff1 * p.x;
			ret.z = fCoeff0 * p.z - fCoeff1 * p.w;
			ret.w = p.z;
		}

		return ret;
	}

    public static float Hermite(float start, float end, float value)
    {
        return Mathf.Lerp(start, end, value * value * (3.0f - 2.0f * value));
    }

    public static float Sinerp(float start, float end, float value)
    {
        return Mathf.Lerp(start, end, Mathf.Sin(value * Mathf.PI * 0.5f));
    }

    public static float Coserp(float start, float end, float value)
    {
        return Mathf.Lerp(start, end, 1.0f - Mathf.Cos(value * Mathf.PI * 0.5f));
    }

    public static float AngleLessThan180(Vector3 vA, Vector3 vB)
    {
        float angle = Vector3.Angle(vA, vB);

        if (angle > 180)
            angle = 360 - angle;

        return angle;
    }

	//-----------------------------------------------------------------------------
	//use a vector to return the angle components
	//quickest angle ( left or right )
	//left angle
	//right angle
	public static Vector3 GetATAN2AngleComponents ( Vector2 a, Vector2 b )
	{
		//ugh and yet another atan2 code... even if i use it every project, i still hate it... (cf)
		a.Normalize();
		b.Normalize();

		//calc atan angle to know if i need to turn
		float left = 0, right = 0, angle = -1;
		{
			//calc atan and calibrate to 0 -> 2*Mathf.PI instead of -Mathf.PI -> to Mathf.PI
			float atanB = Mathf.Atan2( b.y, b.x );
			if ( atanB < 0 )
				atanB += 2 * Mathf.PI;

			float atanA = Mathf.Atan2( a.y, a.x );
			if ( atanA < 0 )
				atanA += 2 * Mathf.PI;

			//what direction?
			if ( atanB > atanA )
			{
				left = atanB - atanA;
				right = 2 * Mathf.PI - atanB + atanA;
			}
			else /*if(atanB <= atanA)*/
			{
				left = 2 * Mathf.PI - atanA + atanB;
				right = atanA - atanB;
			}

			//what is the quickest route?
			if ( left > right )
				angle = right;
			else
				angle = left;
		}

		return new Vector3( angle, left, right );
	}

	//! Rotates the vector by a specified number of degrees around the Y axis and the specified center.
	/** \param degrees Number of degrees to rotate around the Y axis.
		\param center The center of the rotation. */
	public static void rotateXZBy(ref Vector3 v, float degrees)
	{
		Vector3 center = Vector3.zero;
		degrees *= (Mathf.PI / 180f);
		float cs = Mathf.Cos(degrees);
		float sn = Mathf.Sin(degrees);
		v.x -= center.x;
		v.z -= center.z;
		//mbadita fix for rotation
		float n = v.x * cs - v.z * sn;
		v.z = v.x * sn + v.z * cs;
		v.x = n;
		v.x += center.x;
		v.z += center.z;
	}

	public static float ASINF_FROM_VECTS ( Vector3 v1, Vector3 v2 )
	{
		v1.Normalize();
		v2.Normalize();
		float dotProd = Vector3.Dot( v1, v2 );
		return Mathf.Asin( dotProd );
	}

	public static float ACOSF_FROM_VECTS ( Vector3 v1, Vector3 v2 )
	{
		v1.Normalize();
		v2.Normalize();
		float dotProd = Vector3.Dot( v1, v2 );
		return Mathf.Acos( dotProd );
	}

	public static Vector2 GetClosestPoint ( Vector2 lineStart, Vector2 lineEnd, Vector2 point )
	{
		Vector2 c = point - lineStart;
		Vector2 v = lineEnd - lineStart;
		float d = v.magnitude;
		v /= d;
		float t = Vector2.Dot( v, c );

		if ( t < 0f )
			return lineStart;
		if ( t > d )
			return lineEnd;

		v *= t;
		return lineStart + v;
	}

	public static Vector3 GetClosestPoint ( Vector3 lineStart, Vector3 lineEnd, Vector3 point )
	{
		Vector3 c = point - lineStart;
		Vector3 v = lineEnd - lineStart;
		float d = v.magnitude;
		v /= d;
		float t = Vector3.Dot( v, c );

		if ( t < 0f )
			return lineStart;
		if ( t > d )
			return lineEnd;

		v *= t;
		return lineStart + v;
	}
}
